﻿/**
 * @file ut_algorithm.h
 * @author 贺亚猛
 * @brief  实现常用图像等相关算法
 * @version 0.1
 * @date 2022-02-09
 * @copyright Copyright 2022 Langge Software Co., Ltd. All rights reserved.
 */

#ifndef UT_ALGORITHM_H
#define UT_ALGORITHM_H

#include "ut_symbols.h"
#include "ut_point.h"
#include "ut_rect.h"
#include "ut_coordinates.h"
#include <cmath>
#include <math.h>
#include <stdint.h>
#include <vector>
#include <geos.h>
#include <geos/simplify/TopologyPreservingSimplifier.h>

BEGIN_SPACE_LANGE_VDR

/**
 * @brief 实现常用图像等相关算法
 */
class UT_API UTAlgorithm
{
public:
    static constexpr double ALG_PI = 3.14159265358979323846;                             // Π
    static constexpr double ALG_DOUBLE_EPS = 1e-8;                                       // 双精度零值
    static constexpr double ALG_FLOAT_EPS = 1e-6;                                        // 单精度零值
    static constexpr double ALG_LENGTH_PRIME_MERIDIAN = 39940.67;                        // 子午线长，（单位：km）
    static constexpr double ALG_LENGTH_EQUATOR = 40075.36;                               // 赤道长度（单位：km）
    static constexpr double ALG_DIST_LON_PER_DEGREE = ALG_LENGTH_PRIME_MERIDIAN / 360.0; // 纬度改变一度，对应经线的距离，单位：km
    static constexpr double ALG_DIST_LON_PER_CENT = ALG_DIST_LON_PER_DEGREE / 60.0;      // 纬度改变一分，对应经线的距离，单位：km
    static constexpr double ALG_DIST_LON_PER_SEC = ALG_DIST_LON_PER_CENT / 60.0;         // 纬度改变一秒，对应经线的距离，单位：km
    static constexpr double ALG_RATIO_LATLON_TO_DIST = 1e5;                              // 将经纬度坐标转换为距离坐标比例系数
    static constexpr double ALG_RATIO_DTOL = 1.1111111e-5;                               // 1/90000平面坐标到经纬度的转化系数;
    static constexpr double ALG_COEF = 32.287;                                           // （真实经纬度的倍率3600000 * 真实经纬度）与距离（单位：m）的折算率
    // 赤道半径（地球长轴）
    constexpr static const int R = 6378137;
    // 地球短轴
    constexpr static double SHORT_RADIS = 6356752.3142;
    // 椭圆偏心率
    constexpr static double f = (R - SHORT_RADIS) / R;
   
    static constexpr double TEM_F = 2 * f - f * f;

    static constexpr double METERS_PER_DEGREE = (R * ALG_PI / 180.0);

public:
    /**
     * @brief 计算两点间的距离
     * @tparam T     数据类型
     * @param x0     第一点x
     * @param y0     第一点y
     * @param x1     第二点x
     * @param y1     第二点y
     * @return 返回计算结果
     */
    template <typename T>
    static T CalculateDistance(const T x0, const T y0, const T x1, const T y1);
    /**
     * @brief 计算两点间距离的平方
     * @tparam T    数据类型
     * @param  x0   第一点x
     * @param  y0   第一点y
     * @param  x1   第二点x
     * @param  y1   第二点y
     * @param rate  误差率（没有误差时，填1）
     * @return 返回计算结果
     */
    template <typename T>
    static T CalculateSquareDistance(const T x0, const T y0, const T x1,
                                     const T y1, const T rate = static_cast<T>(1));
    /**
     * @brief 计算四面体体积
     * @tparam T  -  参数类型
     * @param  a  -  四面体第一个点
     * @param  b  -  四面体第二个点
     * @param  c  -  四面体第三个点
     * @param  d  -  四面体第四个点
     * @return 返回四面体体积
     */
    template <typename T>
    static float ComputeVolume(const T &a, const T &b, const T &c, const T &d);
    /**
     * @brief 计算三棱柱体积(将其抛分为3个四面体)
     * @tparam T  - 参数类型
     * @param f1  - 三棱柱第一个点
     * @param f2  - 三棱柱第二个点
     * @param f3  - 三棱柱第三个点
     * @param b1  - 三棱柱第四个点
     * @param b2  - 三棱柱第五个点
     * @param b3  - 三棱柱第六个点
     * @return 返回三棱柱体积
     */
    template <typename T>
    static float ComputeVolume(const T &f1, const T &f2, const T &f3,
                               const T &b1, const T &b2, const T &b3);

public:
    /**
     * @brief 计算两个角度的差值
     * @param angle1   角度1（单位：弧度）
     * @param angle2   角度2（单位：弧度）
     * @return 返回角度差
     */
    static float AngleDiff(const float angle1, const float angle2);
    /**
     * @brief 计算两个角度的差值
     * @param angle1   角度1（单位：弧度）
     * @param angle2   角度2（单位：弧度）
     * @return 返回角度差
     */
    static double AngleDiff(const double angle1, const double angle2);
    /**
     * @brief 点与直线位置关系
     * @param point         点
     * @param lineStart     直线起点
     * @param lineEnd       直线终点
     * @return >0           点在直线上面
     *         <0           点在直线下面
     *         0            点在直线上
     */
    static int32_t PointLineStation(const MI2DPOINT &point, const MI2DPOINT &lineStart, const MI2DPOINT &lineEnd);
    /**
     * @brief 点与直线位置关系
     * @param point         点
     * @param lineStart     直线起点
     * @param lineEnd       直线终点
     * @return >0           点在直线外部
     *         <0           点在直线内部
     *         0            点在直线上
     */
    static int32_t PointLineStation(const MD2DPOINT &point, const MD2DPOINT &lineStart, const MD2DPOINT &lineEnd);
    /**
     * @brief 获取线A、线B的交点
     * @param clzLineAStart     线A起点
     * @param clzLineAEnd       线A终点
     * @param clzLineBStart     线B起点
     * @param clzLineBEnd       线B终点
     * @param clzIntersection   返回交点
     * @return true             函数执行成功
     * @return false            函数执行失败
     */
    static bool GetIntersectionOfLines(const MD2DPOINT &clzLineAStart, const MD2DPOINT &clzLineAEnd,
                                       const MD2DPOINT &clzLineBStart, const MD2DPOINT &clzLineBEnd,
                                       MD2DPOINT &clzIntersection);
    /**
     * @brief 用四边形裁剪直线
     * @param clzLeftTop        四边形 left、top 顶点
     * @param clzRightTop       四边形 right、top 顶点
     * @param clzRightBottom    四边形 right、bottom 顶点
     * @param clzLeftBottom     四边形 left、bottom 顶点
     * @param[in out] clzLineStart      直线起点
     * @param[in out] clzLineEnd        直线终点
     * @return >0               直线已被成功裁剪，通过 clzLineStart、clzLineEnd 返回
     * @return 0                直线在矩形外应该丢弃
     * @return <0               剪裁失败，函数执行错误
     */
    static int32_t ClipLineWidthQuad(const MD2DPOINT &clzLeftTop, const MD2DPOINT &clzRightTop,
                                     const MD2DPOINT &clzRightBottom, const MD2DPOINT &clzLeftBottom,
                                     MD2DPOINT &clzLineStart, MD2DPOINT &clzLineEnd);
    /**
     * @brief 点与三角形位置关系
     * @param point        点
     * @param triOne       三角形第一个点
     * @param triTwo       三角形第二个点
     * @param triThree     三角形第三个点
     * @return >0          点在三角形外
     *         <0          点在三角形内
     *         ==0         在三角形上
     */
    static int32_t PointTriangleStation(const MI2DPOINT &point, const MI2DPOINT &triOne,
                                        const MI2DPOINT &triTwo, const MI2DPOINT &triThree);
    /**
     * @brief 点与矩形位置关系
     * @param pt        点
     * @param rect      矩形
     * @return true     点在矩形内
     * @return false    点在矩形外
     */
    static bool PointRectangleStation(const MI2DPOINT &pt, const MIUTRECT &rect);
    /**
     * @brief 点与多边形位置关系
     * @param point       点
     * @param vertexes    多边形顶点数组
     * @param count       多边形顶点个数
     * @return true       点在多边形内
     * @return false      点在多边形外
     */
    static bool PointInPolygon(const MI2DPOINT &point, const MI2DPOINT *vertexes, const int32_t count);
    /**
     * @brief 计算点到直线距离的平方，并求出垂足
     * @param point             点       (真实经纬度乘以3600000.0)
     * @param lineStart         直线起点  (真实经纬度乘以3600000.0)
     * @param lineEnd           直线终点  (真实经纬度乘以3600000.0)
     * @param rate              误差率    (没有误差就填1)
     * @param verticalPoint     垂足点    (真实经纬度乘以3600000.0)
     * @return 返回颠倒直线距离的平方，通过参数返回垂足
     */
    static float Point2LineDistance(const MI2DPOINT &point, const MI2DPOINT &lineStart,
                                    const MI2DPOINT &lineEnd, const float rate, MI2DPOINT &verticalPoint);
    /**
     * @brief 计算点到直线垂直距离的平方，并求出垂足
     * @param Point               点       屏幕象素坐标
     * @param VerticalPoint       垂足点   屏幕象素坐标
     * @param lineStart           直线起点 屏幕象素坐标
     * @param lineEnd             直线终点 屏幕象素坐标
     * @param rate                误差率(没有误差就填为1)
     * @return 点到直线垂直距离的平方和垂足点
     */
    static float Point2LineDis(const MF2DPOINT &Point, MF2DPOINT &VerticalPoint,
                               const MF2DPOINT &lineStart, const MF2DPOINT &lineEnd, const float rate);
    /**
     * @brief 求点到曲线垂直距离的平方,并求出垂足
     *
     * @param Point               点(绝对坐标)            真实经纬度乘以3600000.0
     * @param VerticalPoint       垂足点(返回值 绝对坐标)  真实经纬度乘以3600000.0
     * @param nCoorOrder          垂足在曲线第几个点后面(返回值 如果在延长线上,则返回-1)
     * @param pPotArray           曲线点集 (绝对坐标)     真实经纬度乘以3600000.0
     * @param lSizeOfPotArray     曲线点数量
     * @param rate                误差率(没有误差就填为1)
     * @return 点到曲线垂直距离的平方、垂足点、垂足在曲线第几个点后面 真实经纬度乘以3600000.0
     */
    static float Point2CurveDis(const MI2DPOINT &Point, MI2DPOINT &VerticalPoint, int32_t &nCoorOrder,
                                MI2DPOINT *pPotArray, const long lSizeOfPotArray, const float rate);
    /**
     * @brief 将一个点插入一条曲线中,求出它在哪个曲线点的后面
     * @param pPotArray              曲线点数组指针
     * @param lSizeOfPotArray        曲线点数量
     * @param pot                    插入的点
     * @return -1                    没有找到合适的曲线点
     * @return !=-1                  找到合适的曲线点,返回曲线点的索引
     */
    static int64_t GetInsertPotPlace(const MI2DPOINT *pPotArray, const int64_t lSizeOfPotArray, const MI2DPOINT &pot);
    /**
     * @brief 计算两点连线的角度
     * @param dwStartLon    点1经度 真实经纬度乘以3600000.0
     * @param dwStartLat    点1纬度 真实经纬度乘以3600000.0
     * @param dwEndLon      点2经度 真实经纬度乘以3600000.0
     * @param dwEndLat      点2纬度 真实经纬度乘以3600000.0
     * @return 与正北方向(顺时针旋转)之间的夹角(弧度)
     */
    static double CalcAngle(const int32_t dwStartLon, const int32_t dwStartLat, const int32_t dwEndLon, const int32_t dwEndLat);
    /**
     * @brief 求点到直线的垂足
     * @param Line    直线点坐标列表  真实经纬度乘以3600000.0
     * @param PtA     点             真实经纬度乘以3600000.0
     * @param PtB     存储垂足点     真实经纬度乘以3600000.0
     */
    static int32_t Point2LineVertical(const int32_t (&Line)[4], const int32_t (&PtA)[2], int32_t (&PtB)[2]);

    /**
     * @brief 相应经度改变一度对应纬线的距离
     * @param dLati       经度
     * @return 相应经度改变一度对应纬线的距离，单位：km
     */
    static double DistLatiPerDegree(const double dLati);
    /**
     * @brief 相应经度改变一分对应纬线的距离
     * @param dLati 经度
     * @return 相应经度改变一分对应纬线的距离，单位：km
     */
    static double DistLatiPerCent(const double dLati);
    /**
     * @brief 相应经度改变一秒对应纬线的距离
     * @param dLati     经度
     * @return 相应经度改变一秒对应纬线的距离，单位：km
     */
    static double DistLatiPerSec(const double dLati);
    /**
     * @brief 两点之间的球面距离
     * @param x1         点1经度 DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param y1         点1纬度 DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param x2         点2经度 DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param y2         点2纬度 DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param div        真实经纬度的倍率
     *                   3600000.0  真实经纬度乘以3600000.0(DOOG二进制地图数据输出是乘以3600000.0)
     *                   1000000.0  真实经纬度乘以1000000.0(COMGPS数据输出是乘以1000000.0)
     * @return 两点之间的球面距离，单位：km
     */
    static double GetMapDistance(const int32_t x1, const int32_t y1, const int32_t x2, const int32_t y2, const double div);

    /**
     * @brief 直线差值
     * @param iStartX                      直线起点经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param iStartY                      直线起点纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param iEndX                        直线终点经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param iEndY                        直线终点纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param dInterpolationInterval       差值间隔（单位: km）
     * @param iOutX                        返回插值经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param iOutY                        返回差值纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param dDiv                         真实经纬度的倍率
     *                                     3600000.0  真实经纬度乘以3600000.0(DOOG二进制地图数据输出是乘以3600000.0)
     *                                     1000000.0  真实经纬度乘以1000000.0(COMGPS数据输出是乘以1000000.0)
     * @return true                        函数执行成功
     * @return false                       函数执行失败
     */
    static bool GetLineInterpolation(const int32_t iStartX, const int32_t iStartY, const int32_t iEndX,
                                     const int32_t iEndY, const double dInterpolationInterval,
                                     const double dDiv, int32_t &iOutX, int32_t &iOutY);

    /**
     * @brief 直线差值
     * @param iStartX                      直线起点经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param iStartY                      直线起点纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param iEndX                        直线终点经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param iEndY                        直线终点纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param dLineLen                     线长度（单位：km）
     * @param dInterpolationInterval       差值间隔（单位: km）
     * @param iOutX                        返回插值经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param iOutY                        返回差值纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
     * @param dDiv                         真实经纬度的倍率
     *                                     3600000.0  真实经纬度乘以3600000.0(DOOG二进制地图数据输出是乘以3600000.0)
     *                                     1000000.0  真实经纬度乘以1000000.0(COMGPS数据输出是乘以1000000.0)
     * @return true                        函数执行成功
     * @return false                       函数执行失败
     */
    static bool GetLineInterpolation(const int32_t iStartX, const int32_t iStartY, const int32_t iEndX,
                                     const int32_t iEndY, const double dLineLen, const double dInterpolationInterval,
                                     const double dDiv, int32_t &iOutX, int32_t &iOutY);

    /**
     * @brief 计算曲线的球面长度
     * @param linePnt   曲线的坐标指针 DOOG数据乘以3600000.0 GPS数据乘以100000.0
     * @param pntcnt    曲线的坐标数量
     * @param div       真实经纬度的倍率
     *                  3600000.0  真实经纬度乘以3600000.0(DOOG二进制地图数据输出是乘以3600000.0)
     *                  100000.0   真实经纬度乘以100000.0(COMGPS数据输出是乘以100000.0)
     * @return 曲线的球面长度，单位：km
     */
    static double GetCurveMapDistance(const MI2DPOINT *linePnt, const int32_t pntcnt, const double div);
    /**
     * @brief 将数学角度转换成地理角度(与正北方向顺时针方向的夹角)
     * @param dAngle    数学角度
     * @return 地理角度(弧度)
     */
    static double TransferAngleToGeo(const double dAngle);
    /*
     * @brief 线抽稀
     * @param InLine          抽稀前线         （真实经纬度乘以3600000.0）
     * @param nInLinePNum     抽稀前线的点数量
     * @param OutLine         抽稀后线         （真实经纬度乘以3600000.0）
     * @param nOutLinePNum    抽稀后线的点数量
     * @param nAbstractValue  抽稀参数 指定抽稀后线的点数量
     */
    static void AbstractLine(const MI2DPOINT *InLine, const int32_t nInLinePNum, MI2DPOINT *OutLine,
                             int32_t &nOutLinePNum, const int32_t nAbstractValue);
    /**
     * @brief 线抽稀
     * @param InLine          抽稀前线         屏幕象素坐标
     * @param nInLinePNum     抽稀前线的点数量
     * @param OutLine         抽稀后线(返回值) 屏幕象素坐标
     * @param nOutLinePNum    抽稀后线的点数量(返回值)
     * @param nAbstractValue  抽稀参数 指定抽稀后线的点数量
     */
    static void AbstractLine(const MF2DPOINT *InLine, const int32_t nInLinePNum,
                             MF2DPOINT *OutLine, int32_t &nOutLinePNum, const int32_t nAbstractValue);
    /**
     * @brief 将一个点绕中心点旋转,求出旋转后的点坐标
     * @param CenterPoint     中心点
     * @param Point           点
     * @param RotatePoint     旋转后的点
     * @param nAngle          旋转角度
     */
    static void RotatePoint(const MI2DPOINT &CenterPoint, const MF2DPOINT Point,
                            MF2DPOINT &RotatePoint, const int32_t nAngle);
    /**
     * @brief 求两点间近似平面距离
     * @param pntS     起点
     * @param pntE     终点
     * @return 两点间距离
     */
    static double ApproximateDistance(const MF2DLONLAT &pntS, const MF2DLONLAT &pntE);
    /**
     * @brief 沿着直线pntStart-pntEnd的方向, 在此直线上找一个点, 到起始点(pntStart)的距离等于dDistance。
     * @param pntStart     起点
     * @param pntEnd       终点
     * @param dDistance    查找距离
     * @return 找到的点
     */
    static MF2DLONLAT FindPoint(const MF2DLONLAT &pntStart, const MF2DLONLAT &pntEnd, const double dDistance);
    /**
     * @brief 获取数组中离端点一定距离的大概点数组
     * @param arLinePoints             结果点串
     * @param pPoints                  输入点串
     * @param nPntCount                输入点串个数
     * @param dDistance                查找距离
     * @param bDir                     是否按点串方向正向查找
     */
    static void FindApproximatePointData(std::vector<MF2DLONLAT> &arLinePoints, const MF2DLONLAT *pPoints,
                                         const int32_t nPntCount, const double dDistance, const bool bDir);
    /**
     * @brief 断点是否在复杂多边形内部
     * @param point            被判断的点
     * @param pPoints          复杂多边形点串
     * @param plPolyCounts     复杂多边形点串个数
     * @return true            点在多边形内部
     * @return false           点在复杂多边形外部
     */
    static bool IsWithin(const MI2DPOINT &point, const MI2DPOINT *pPoints, const int32_t plPolyCounts);
    /**
     * @brief 用多边形裁剪多边形（注意：被裁剪多边形和裁剪多边形的点串顺序必须保持一致，要么都是顺时针要么都是逆时针）
     * @param arResultPoints        裁剪后结果多边形组点串[out]
     * @param arResultPntCount      裁剪后结果多边形组内每个多边形点个数的数组[out]
     * @param iResultPolyCount      裁剪后多边形的个数[out]
     * @param pRgnPoints            被裁剪多边形点串[in]
     * @param iRgnPointCount        被裁剪多边形点串个数[in]
     * @param pClipper              裁剪多边形点串[in]
     * @param iCliperCount          裁剪多边形点串个数[in]
     * @param cType                 裁剪的方式（1：求交集；2：求差集；3：求并集）[in]
     * @param isClockWise           裁剪多边形和被裁剪多边形点串的排列顺序（true：顺时针；false：逆时针）[in]
     * @return true                 裁剪成功
     * @return false                裁剪失败
     */
    static bool ClipRegionbyRegion(std::vector<MI2DPOINT> &arResultPoints, std::vector<int32_t> &arResultPntCount,
                                   int32_t &iResultPolyCount, const MI2DPOINT *pRgnPoints, const int32_t iRgnPointCount,
                                   const MI2DPOINT *pClipper, const int32_t iCliperCount, const char cType, const bool isClockWise);
    /**
     * @brief 判断多边形点串排列顺序（只能判断凸多边形）
     * @param pRgn       被多边形点串[in]
     * @param iCount     被多边形点串个数[in]
     * @return -1        顺时针
     * @return  1        逆时针
     * @return  0        在一个点上
     */
    static int32_t IsClockWiseConvex(const MI2DPOINT *pRgn, const int32_t iCount);
    /**
     * @brief 任意多边形方向的判断，包括凹凸多边形。
     * @param pRgn       被多边形点串[in]
     * @param iCount     被多边形点串个数[in]
     * @return  -1       顺时针
     * @return   1       逆时针
     * @return   0       在一个点上
     */
    static int32_t IsClockWise(const MI2DPOINT *pRgn, const int32_t iCount);
    /**
     * @brief 线、面数据抽稀(道格拉斯-普克算法)
     * @param src          抽吸前点串[in]
     * @param dst          抽吸后点串[out]
     * @param tolerance    抽稀精度[in]
     */
    static void DouglasReduction(const std::vector<MI2DPOINT> &src,
                                 std::vector<MI2DPOINT> &dst, const double tolerance);
    /**
     * @brief 计算任意多边形面积
     *         (此接口仅适用于DOOG二进制地图数据输出，即真实经纬度倍率固定为3600000.0)
     *         (请按顺时针或逆时针顺序提供多边形的各个顶点)
     * @param pPolygonPoints   多边形的顶点集合（首指针）(* 3600000)
     * @param nPointCount      多边形的顶点数目
     * @return 多边形的面积，单位：m * m ， 返回0表示计算失败
     */
    static double CalcPolygonArea(const MI2DPOINT *pPolygonPoints, const int32_t nPointCount);
 
    /*角度转弧度*/
    static double ToRadians(double angle) {
        return angle * ALG_PI / 180.0;
    }
    /*弧度转角度*/
    template<typename T>
    static T ToDegrees(T angle) {
        return angle * 180 / ALG_PI;
    }

    static double ConvertNorthZeroToEastZero(double x){
        x = 450 -x;
        x = fmod(x,360);
        return x;
    }
    /*角度（弧度）归一化到(0,2PI)*/
    static double NormalizeAngleRad(double x) {
        x = fmod(x, 2 * ALG_PI);
        if (x > 2 * ALG_PI) x -= 2 * ALG_PI;
        if (x <= 0) x += 2 * ALG_PI;
        return x;
    }
    /*角度（度数）归一化到(0,360]*/
    static double NormalizeDegrees(double x) {
        x = fmod(x, 360.0);
        if (x > 360.0) x -= 360.0;
        if (x <= 0) x += 360.0;
        return x;
    }

    /*计算角度差，有正负*/
    static double CalcDegreeDiff(double x, double y) {
        double diff = x - y;
        diff = fmod(diff, 360);
        if (diff > 180) diff -= 360;
        if (diff < -180) diff += 360;
        return abs(diff);
    }


    static void CalcFootToLineSegment(std::vector<double>& result,
        double x, double y,
        double x1, double y1,
        double x2, double y2);

    /**
 * @brief 经纬度坐标转平面坐标
 * @param result  返回值
 * @param lat 要转换的点的纬度
 * @param lng 要转换的点的经度
 * @param lat0 参考点的纬度
*  @param lng0 参考点的精度
*  @param psi0 参考点的高程
 */
    static void LlaToFlat(std::vector<double>& result, double lat, double lng, double lat0, double lng0, double psi0);

    /**
     * 平面坐标转经纬度坐标
     * @param result
     * @param flat_x
     * @param flat_y
     * @param lat0
     * @param lng0
     * @param psi0
     */
    static void FlatToLla(std::vector<double>& result, double flat_x, double flat_y, double lat0, double lng0, double psi0);

    static void Lla2Ecef(std::vector<double>& res, double lat, double lon, double h);

    static void  Ecef2Lla(std::vector<double>& res, double x, double y, double z);

    static void Ecef2Enu(std::vector<double>& res, double x, double y, double z, double lat0, double lon0, double h0);

    static void  Enu2Ecef(std::vector<double>& res, double xEast, double yNorth, double zUp, double lat0, double lon0, double h0);

    static void Lla2Enu(std::vector<double>& res, double  lat, double lon, double h, double lat_ref, double lon_ref, double h_ref);

    static void  Enu2Lla(std::vector<double>& res, double xEast, double yNorth, double zUp, double lat_ref, double lon_ref, double h_ref);

    static void LngLat2Meter(double &x, double &y) ;

    static void Meter2LngLat(double& x, double& y);
    
    /**
  * @brief 计算两个矩形是否相交
  * @param Src   矩形一
  * @param Dst   矩形二
  * @return true         相交
  * @return false      不相交
  */
    static bool InnerVpIsRectIntersects(const MIUTRECT &Src, const MIUTRECT &Dst);
    /**
     * @brief 计算线段与正北方夹角
     * @param pointS   线段起始点
     * @param pointE   线段终点
     * @return 返回夹角
     */
    static float InnerVpLineAngleFromNorth(const MI2DPOINT &pointS, const MI2DPOINT &pointE);
    /**
     * @brief 线、面数据抽稀(TopologyPreservingSimplifier)
     * @param src          抽吸前点串[in]
     * @param dst          抽吸后点串[out]
     * @param tolerance    抽稀阈值[in]
     */
    template <typename T>
    static void TopologyPreservingSimplifier(const std::vector<UT2DPoint<T>> &src,
                                             std::vector<UT2DPoint<T>> &dst, const T tolerance);
    /**
     * @brief 
     * 
     * @tparam T               数据类型
     * @param src              需要处理的线
     * @param bufferResult     线buffer结果
     * @param distance         buffer半径
     * @return true 
     * @return false 
     */
    template <typename T>
    static bool Buffer(const std::vector<UT2DPoint<T>> &src,
                         std::vector<UT2DPoint<T>> &bufferResult, const T distance);

    /* 计算 模长*/
    static float CalcNorm(const std::vector<float> &nums);

    static std::vector<float> Rotate(std::vector<float> roationMatrix,std::vector<float> &data);

    template<typename T>
    static std::vector<T> RotationMatrix2EulerAngle(std::vector<T> const &rotationMatrix);

    template<typename T>
    static std::vector<T> Quaternion2RotationMatrix(std::vector<T> const &quaternion);

    template<typename T>
    static std::vector<T> Quaternion2EulerAngle(std::vector<T> const &quaternion);


private:
    UTAlgorithm() = delete;
    UTAlgorithm(UTAlgorithm &&) = delete;
    UTAlgorithm(const UTAlgorithm &) = delete;
    UTAlgorithm &operator=(UTAlgorithm &&) = delete;
    UTAlgorithm &operator=(const UTAlgorithm &) = delete;


};

/**
 * @brief 计算两点间的距离
 * @tparam T     数据类型
 * @param x0     第一点x
 * @param y0     第一点y
 * @param x1     第二点x
 * @param y1     第二点y
 * @return 返回计算结果
 */
template <typename T>
T UTAlgorithm::CalculateDistance(const T x0, const T y0, const T x1, const T y1)
{
    return std::sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1));
}

/**
 * @brief 计算两点间距离的平方
 * @tparam T    数据类型
 * @param  x0   第一点x
 * @param  y0   第一点y
 * @param  x1   第二点x
 * @param  y1   第二点y
 * @param rate  误差率（没有误差时，填1）
 * @return 返回计算结果
 */
template <typename T>
T UTAlgorithm::CalculateSquareDistance(const T x0, const T y0, const T x1, const T y1,
                                       const T rate /*=static_cast<T>(1)*/)
{
    return (x0 - x1) * (x0 - x1) * rate + (y0 - y1) * (y0 - y1);
}

/**
 * @brief 计算四面体体积
 * @tparam T  -  参数类型
 * @param  a  -  四面体第一个点
 * @param  b  -  四面体第二个点
 * @param  c  -  四面体第三个点
 * @param  d  -  四面体第四个点
 * @return 返回四面体体积
 */
template <typename T>
float UTAlgorithm::ComputeVolume(const T &a, const T &b, const T &c, const T &d)
{
    return fabsf((float)(((b - c) ^ (a - b)) * (d - b)));
}

/**
 * @brief 计算三棱柱体积(将其抛分为3个四面体)
 * @tparam T  - 参数类型
 * @param f1  - 三棱柱第一个点
 * @param f2  - 三棱柱第二个点
 * @param f3  - 三棱柱第三个点
 * @param b1  - 三棱柱第四个点
 * @param b2  - 三棱柱第五个点
 * @param b3  - 三棱柱第六个点
 * @return 返回三棱柱体积
 */
template <typename T>
float UTAlgorithm::ComputeVolume(const T &f1, const T &f2, const T &f3,
                                 const T &b1, const T &b2, const T &b3)
{
    return ComputeVolume(f1, f2, f3, b1) +
           ComputeVolume(b1, b2, b3, f2) +
           ComputeVolume(b1, b3, f2, f3);
}

/**
 * @brief 线、面数据抽稀(TopologyPreservingSimplifier)
 * @param src          抽吸前点串[in]  (单位：经纬度坐标)
 * @param dst          抽吸后点串[out] (单位：经纬度坐标)
 * @param tolerance    抽稀阈值[in]    (单位：米)
 */
template <typename T>
void UTAlgorithm::TopologyPreservingSimplifier(const std::vector<UT2DPoint<T>> &src,
                                               std::vector<UT2DPoint<T>> &dst, const T tolerance)
{
    GeometryFactory::Ptr geoFactory;
    geoFactory = GeometryFactory::create();
    CoordinateArraySequence::Ptr cs = CoordinateArraySequence::Ptr(new CoordinateArraySequence());
    std::vector<Coordinate> sourcePoints;
    for (auto point : src)
    {
        UTCoordinates::ProjPoint mp = UTCoordinates::LonLatToProj(point.GetX(), point.GetY());
        sourcePoints.push_back(Coordinate(mp.GetX(), mp.GetY()));
    }
    cs->setPoints(sourcePoints);
    LineString *lineString = geoFactory->createLineString(cs.release());
    if (lineString)
    {
        geos::geom::Geometry::Ptr simplified = geos::simplify::TopologyPreservingSimplifier::simplify(lineString, tolerance);
        std::unique_ptr<CoordinateSequence> simplifyCS = simplified->getCoordinates();
        if (simplifyCS)
        {
            for (size_t i = 0; i < simplifyCS->getSize(); i++)
            {
                Coordinate coord = simplifyCS->getAt(i);
                UTCoordinates::LonLatPoint geoPoint = UTCoordinates::ProjToLonLat(coord.x, coord.y);
                dst.push_back(UT2DPoint<T>((T)geoPoint.GetX(), (T)geoPoint.GetY()));
            }
        }
    }
    geoFactory->destroyGeometry(lineString);
}


template <typename T>
bool UTAlgorithm::Buffer(const std::vector<UT2DPoint<T>> &src,
                         std::vector<UT2DPoint<T>> &bufferResult, const T distance)
{
    GeometryFactory::Ptr geoFactory;
    geoFactory = GeometryFactory::create();
    CoordinateArraySequence::Ptr cs = CoordinateArraySequence::Ptr(new CoordinateArraySequence());
    std::vector<Coordinate> sourcePoints;
    for (auto point : src)
    {
        sourcePoints.push_back(Coordinate(point.GetX(), point.GetY()));
    }
    cs->setPoints(sourcePoints);
    LineString *lineString = geoFactory->createLineString(cs.release());
    if (lineString)
    {
        try {
            geos::geom::Geometry::Ptr buffer = lineString->buffer(distance);
            std::unique_ptr<CoordinateSequence> bufferCS = buffer->getCoordinates();
            if (bufferCS)
            {
                for (size_t i = 0; i < bufferCS->getSize(); i++)
                {
                    Coordinate coord = bufferCS->getAt(i);
                    bufferResult.push_back(UT2DPoint<T>((T)coord.x, (T)coord.y));
                }
            }
		}
		catch (const geos::util::GEOSException& exc) {
			std::cerr <<"GEOS Exception: lineString->buffer(): "<<exc.what()<<"\n";
            return false;
		}
        
    }
    geoFactory->destroyGeometry(lineString);
    return true;
}

END_SPACE_LANGE_VDR
#endif // !UT_ALGORITHM_H